#include<iostream>
#include<string>
#include <stdlib.h>
#include <stdio.h>
#include <sstream>
#include <cmath>

#include <ros/ros.h>
#include <ros/spinner.h>

#include<tf/transform_broadcaster.h>
#include<nav_msgs/Odometry.h>
#include<geometry_msgs/Twist.h>

#include <serial/serial.h>
#include <std_msgs/String.h>

#include <boost/asio.hpp>                  //包含boost库函数
#include <boost/bind.hpp>
#include <sys/time.h>

#include <iostream>
#include <fstream>
#include <sstream>
#include <signal.h>


using namespace std;
using namespace boost::asio;           //定义一个命名空间，用于后面的读写操作

float WHEEL_K=0.355;        // abs(X) + abs(Y) 轮距
float WHEEL_D=0.22; 			//轮子直径
float WHEEL_R=WHEEL_D/2.0; 			//轮子直径
float WHEEL_PI=3.141693; 			//pi

float WHEEL_RATIO = 819672; // Wheel ration from velocity (m/s) to ATEC robot platform motor command

struct timeval time_val; //time varible
struct timezone tz;
double time_stamp;
serial::Serial ros_ser;
ros::Publisher odom_pub;
 
void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg);
void send_speed_to_chassis(float x,float y,float w);
void send_rpm_to_chassis( int w1, int w2, int w3, int w4);
bool analy_uart_recive_data( std_msgs::String serial_data);

unsigned int accum = 0;
unsigned int Gr1 = 0x0810;
float maxVal = 1280000;
uint8_t wheel1_data_tem[50];
uint8_t wheel2_data_tem[50];


void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg);
void ResetCRC();
void CrunchCRC (char x);
int hex2int(char c);
void crccheck(uint8_t data, uint8_t *accumulator, uint8_t *crctable);
int vel_normalized(float vel);
void print_wheel_serial_command();
bool init_wheels();
void MySigintHandler(int sig);
void write2serials();
void send_vel2wheel(float v_1, float v_2);



int main(int argc,char** argv)
{
  string out_result;
  bool uart_recive_flag;
  
  string sub_cmdvel_topic,pub_odom_topic,dev;
	int buad,time_out,hz;
 	ros::init(argc, argv, "mickrobot");
	ros::NodeHandle n;
	signal(SIGINT, MySigintHandler);
 
	n.param<std::string>("sub_cmdvel_topic", sub_cmdvel_topic, "/cmd_vel");
	n.param<std::string>("pub_odom_topic", pub_odom_topic, "/odom");
	n.param<std::string>("dev", dev, "/dev/platform");
	n.param<int>("buad", buad, 115200);
	n.param<int>("time_out", time_out, 1000);
	n.param<int>("hz", hz, 100);
	
	ROS_INFO_STREAM("sub_cmdvel_topic:   "<<sub_cmdvel_topic);
	ROS_INFO_STREAM("pub_odom_topic:   "<<pub_odom_topic);
	ROS_INFO_STREAM("dev:   "<<dev);
	ROS_INFO_STREAM("buad:   "<<buad);
	ROS_INFO_STREAM("time_out:   "<<time_out);
	ROS_INFO_STREAM("hz:   "<<hz);
 
////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////





	
	 //订阅主题command
  ros::Subscriber command_sub = n.subscribe(sub_cmdvel_topic, 10, cmd_vel_callback);
// 开启串口模块
  try
  {
    ros_ser.setPort(dev);
    ros_ser.setBaudrate(buad);
    //ros_serial::Timeout to = serial::Timeout::simpleTimeout(1000);
    serial::Timeout to = serial::Timeout::simpleTimeout(time_out);
    ros_ser.setTimeout(to);
    ros_ser.open();
    ros_ser.flushInput(); //清空缓冲区数据
  }
  catch (serial::IOException& e)
  {
    ROS_ERROR_STREAM("Unable to open port ");
    return -1;
  }

	if(ros_ser.isOpen())
	{
	  ros_ser.flushInput(); //清空缓冲区数据
	  ROS_INFO_STREAM("Serial Port opened");
	}
	else
	{
	  return -1;
    ROS_ERROR_STREAM("Unable to open port ");
	}
 
  ros::Rate loop_rate(hz);

  bool init_OK=false;

  while(!init_OK)	
  {
    init_OK = init_wheels();
    //ros::spinOnce();
    loop_rate.sleep();
            
  }
  ROS_INFO_STREAM("initialized successful !");
   
  while(ros::ok())
    { 
      ros::spinOnce();
      loop_rate.sleep();
			
    }
   
  ROS_INFO("Node exit");
  return 0;

}

void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{
  //ROS_INFO_STREAM("Recv cmd" << ros::Time::now());
  float speed_x,speed_y,speed_w;
  float v1=0,v2=0;

  speed_x = msg->linear.x;
  speed_y = msg->linear.y;
  speed_w = msg->angular.z;
  
  v1 =speed_x-WHEEL_K*speed_w;       //转化为每个轮子的线速度
  v2 =speed_x+WHEEL_K*speed_w;
  v2 = -v2; // The vel command of v2 is the opsite

  send_vel2wheel(v1, v2);
}




void ResetCRC()
{
  // Resets the Accumulator
  // Call before each new CRC value to calculate
  accum = 0;
}


void CrunchCRC (char x)
  {    // Compute CRC using BitbyBit method
    int i, k;
    for (k=0; k<8; k++) 
    {
      i = (x >> 7) & 1;
      if (accum & 0x8000)
      {
        accum = ((accum ^ Gr1) << 1) + (i ^ 1);
      }
      else
      {
      accum = (accum << 1) + i;
      }
      accum &= 0x0ffff;
      x <<= 1;
    }
  }

int hex2int(char c)
{
	if ((c >= 'A') && (c <= 'Z'))
	{
		return c - 'A' + 10;
	}
	else if ((c >= 'a') && (c <= 'z'))
	{
		return c - 'a' + 10;
	}
	else if ((c >= '0') && (c <= '9'))
	{
		return c - '0';
	}
}


void crccheck(uint8_t data, uint8_t *accumulator, uint8_t *crctable)
{
*accumulator = ( *accumulator << 8 ) ^ crctable[( *accumulator >> 8) ^ data];
}


int vel_normalized(float vel){
  
  vel = vel*WHEEL_RATIO;

  if (abs(vel)>maxVal){
    if (vel>0)
       {vel = maxVal;}
    else {vel = -maxVal;}
  }
  int int_vel = (int)vel;
  //ROS_INFO_STREAM("vel normalized: "<<int_vel);
  return int_vel;

}

void print_wheel_serial_command(){
  int i;
  for (i=0; i<14; i++){
    std::cout <<hex<<int(wheel1_data_tem[i]);
  }
  std::cout<<std::endl;

  for (i=0; i<14; i++){
    std::cout <<hex<<int(wheel2_data_tem[i]);
  }
  std::cout<<std::endl;
}

bool init_wheels(){

  string wheel_1 = "A5011245000228A8000000000000";
  string wheel_2 = "A50222450002EA93000000000000";
  int i;
  int mask = 0xff;
  int val = 0;
  string st;

// set wheel 1 

  st = wheel_1;
  for(i=0; 2*i<st.length(); i++){
      unsigned short int high = hex2int(st[2*i]);
      unsigned short int low = hex2int(st[2*i+1]);
      wheel1_data_tem[i] = high*16+low;
  }

  // set wheel 2
  st = wheel_2;
  for(i=0; 2*i<st.length(); i++){
      unsigned short int high = hex2int(st[2*i]);
      unsigned short int low = hex2int(st[2*i+1]);
      wheel2_data_tem[i] = high*16+low;
  }

  write2serials();
  std::cout<<"Initialized wheels"<<std::endl;
  print_wheel_serial_command();
  return true;
}
void MySigintHandler(int sig)
{
	//这里主要进行退出前的数据保存、内存清理、告知其他节点等工作
	ROS_INFO("Shutting down ATec Mobile Platform!");
  init_wheels();
	ros::shutdown();
}

void write2serials(){

  ros_ser.write(wheel2_data_tem, 14);
  ros_ser.write(wheel1_data_tem, 14);
}

void send_vel2wheel(float v_1, float v_2){

  //string wheel_1 = "A5011245000228A8F0F6FAFF2C45";
  //string wheel_2 = "A50222450002EA93100905007AC3";

  string wheel_1 = "A5011245000228A8000000000000";
  string wheel_2 = "A50222450002EA93000000000000";
  int i;
  int mask = 0xff;
  int val = 0;
  string st;

// set wheel 1 

  st = wheel_1;
  for(i=0; 2*i<st.length(); i++){
      unsigned short int high = hex2int(st[2*i]);
      unsigned short int low = hex2int(st[2*i+1]);
      wheel1_data_tem[i] = high*16+low;
  }
  
  val = vel_normalized(v_1);
  for (i=0;i<4;i++)
  {
      //cout << hex<<a <<endl;
      int j = val & mask;
      wheel1_data_tem[8+i] = j;
      val = val >> 8; 
  }

  // create CRC code
  //ROS_INFO_STREAM("Before crc" << ros::Time::now());
  ResetCRC();
  for (i=8; i<12; i++){
    CrunchCRC(wheel1_data_tem[i]);
  }
  CrunchCRC(0);
  CrunchCRC(0);
  //ROS_INFO_STREAM("After crc" << ros::Time::now());
  //printf("CRC is %04x\n", accum);

  wheel1_data_tem[13] =  accum&0xff;
  accum = accum>>8;
  wheel1_data_tem[12] =  accum&0xff;

  // set wheel 2
  st = wheel_2;
  for(i=0; 2*i<st.length(); i++){
      unsigned short int high = hex2int(st[2*i]);
      unsigned short int low = hex2int(st[2*i+1]);
      wheel2_data_tem[i] = high*16+low;
  }


  val = vel_normalized(v_2);

  for (i=0;i<4;i++)
  {
      //cout << hex<<a <<endl;
      int j = val & mask;
      wheel2_data_tem[8+i] = j;
      val = val >> 8; 
  }

  ResetCRC();

  for (i=8; i<12; i++){
    CrunchCRC(wheel2_data_tem[i]);
  }

  CrunchCRC(0);
  CrunchCRC(0);
  //printf("CRC is %04x\n", accum);

  wheel2_data_tem[13] =  accum&0xff;
  accum = accum>>8;
  wheel2_data_tem[12] =  accum&0xff;

  //print_wheel_serial_command();

  write2serials();

}
